/*
 * Main.cpp
 *
 *  Created on: 4 dec. 2012
 *      Author: Admin
 */

#include "HardwareController.hpp"
#include "Robot.hpp"

__extension__ typedef int __guard __attribute__((mode (__DI__)));

extern "C" int __cxa_guard_acquire(__guard *);
extern "C" void __cxa_guard_release (__guard *);
extern "C" void __cxa_guard_abort (__guard *);
extern "C" void   atexit( void ) { }

int __cxa_guard_acquire(__guard *g) {return !*(char *)(g);};
void __cxa_guard_release (__guard *g) {*(char *)g = 1;};
void __cxa_guard_abort (__guard *) {};

int main(int argc, char **argv)
{
//	while(true)
//		if (HardwareController::getHardwareController().pollEmergencyStop())
//			HardwareController::getHardwareController().activateLED();
//		else
//			HardwareController::getHardwareController().disableLED();
//	while (true)
//	{
//		HardwareController::getHardwareController().activateWheels();
//		if (HardwareController::getHardwareController().pollCollisionSensorBack())
//			HardwareController::getHardwareController().leftWheelForward();
//		else
//			HardwareController::getHardwareController().leftWheelBackward();
//
//		if (HardwareController::getHardwareController().pollCollisionSensorFront())
//			HardwareController::getHardwareController().rightWheelForward();
//		else
//			HardwareController::getHardwareController().rightWheelBackward();
//	}

	Robot::getInstance().init();
	HardwareController::getHardwareController();
	Robot::getInstance().restart();

//	while (true)
//	{
//		HardwareController::getHardwareController().activateWheels();
//		HardwareController::getHardwareController().leftWheelBackward();
//		HardwareController::getHardwareController().rightWheelForward();
//	}
}
